
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       mb_motors_matrix.h
  * @author     baiyang
  * @date       2023-2-1
  ******************************************************************************
  */

#pragma once

#ifdef __cplusplus
extern "C"{
#endif

/*----------------------------------include-----------------------------------*/
#include "mb_motors_multicopter.h"
/*-----------------------------------macro------------------------------------*/
#define MB_MOTORS_MATRIX_YAW_FACTOR_CW   -1
#define MB_MOTORS_MATRIX_YAW_FACTOR_CCW   1
/*----------------------------------typedef-----------------------------------*/
// same structure, but with floats.
struct MotorDef {
    float angle_degrees;
    float yaw_factor;
    uint8_t testing_order;
};

// structure used for initialising motors that add have separate
// roll/pitch/yaw factors.  Note that this does *not* include
// the final parameter for the add_motor_raw call - throttle
// factor as that is only used in the scripting binding, not in
// the static motors at the moment.
struct MotorDefRaw {
    float roll_fac;
    float pitch_fac;
    float yaw_fac;
    uint8_t testing_order;
};

typedef struct mb_motors_matrix* mb_motors_mat_t;
typedef const struct mb_motors_matrix* mb_motors_mat_const_t;
typedef struct mb_motors_matrix_ops* mb_motors_mat_ops_t;

struct mb_motors_matrix {
    struct mb_motors_multicopter  motors_mc;

    mb_motors_mat_ops_t ops_mat;

    float               _roll_factor[MB_MOTORS_MAX_NUM_MOTORS]; // each motors contribution to roll
    float               _pitch_factor[MB_MOTORS_MAX_NUM_MOTORS]; // each motors contribution to pitch
    float               _yaw_factor[MB_MOTORS_MAX_NUM_MOTORS];  // each motors contribution to yaw (normally 1 or -1)
    float               _throttle_factor[MB_MOTORS_MAX_NUM_MOTORS];  // each motors contribution to throttle 0~1
    float               _thrust_rpyt_out[MB_MOTORS_MAX_NUM_MOTORS]; // combined roll, pitch, yaw and throttle outputs to motors in 0~1 range
    uint8_t             _test_order[MB_MOTORS_MAX_NUM_MOTORS];  // order of the motors in the test sequence

    // motor failure handling
    float               _thrust_rpyt_out_filt[MB_MOTORS_MAX_NUM_MOTORS];    // filtered thrust outputs with 1 second time constant
    uint8_t             _motor_lost_index;  // index number of the lost motor

    enum mb_motor_frame_class   _active_frame_class; // active frame class (i.e. quad, hexa, octa, etc)
    enum mb_motor_frame_type    _active_frame_type;  // active frame type (i.e. plus, x, v, etc)

    const char*         _frame_class_string; // string representation of frame class
    const char*         _frame_type_string;  //  string representation of frame type
};

struct mb_motors_matrix_ops {
    // configures the motors for the defined frame_class and frame_type
    void        (*setup_motors)(mb_motors_t motors, enum mb_motor_frame_class frame_class, enum mb_motor_frame_type frame_type);
};
/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void                mb_motors_matrix_ctor(mb_motors_mat_t motors_matrix, mb_motors_mat_ops_t ops_mat, mb_motors_mc_ops_t ops_mc, mb_motors_ops_t ops, uint16_t speed_hz);

// output_test_num - spin a motor connected to the specified output channel
//  (should only be performed during testing)
//  If a motor output channel is remapped, the mapped channel is used.
//  Returns true if motor output is set, false otherwise
//  pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000
bool                mb_motors_output_test_num(mb_motors_t motors, uint8_t output_channel, int16_t pwm);

// add_motor using raw roll, pitch, throttle and yaw factors
// default, float throttle_factor = 1.0f
void                mb_motors_add_motor_raw(mb_motors_t motors, int8_t motor_num, float roll_fac, float pitch_fac, float yaw_fac, uint8_t testing_order, float throttle_factor);
void                mb_motors_add_motors_raw(mb_motors_t motors, const struct MotorDefRaw *motors_def_raw, uint8_t num_motors);

// method to add many motors specified in a structure:
void                mb_motors_add_motors(mb_motors_t motors, const struct MotorDef *motors_def, uint8_t num_motors);

// add_motor using just position and yaw_factor (or prop direction)
void                mb_motors_add_motor(mb_motors_t motors, int8_t motor_num, float angle_degrees, float yaw_factor, uint8_t testing_order);

// remove_motor
void                mb_motors_remove_motor(mb_motors_t motors, int8_t motor_num);

// add_motor using separate roll and pitch factors (for asymmetrical frames) and prop direction
void                mb_motors_add_motor2(mb_motors_t motors, int8_t motor_num, float roll_factor_in_degrees, float pitch_factor_in_degrees, float yaw_factor, uint8_t testing_order);

// check for failed motor
//   should be run immediately after output_armed_stabilizing
//   first argument is the sum of:
//      a) throttle_thrust_best_rpy : throttle level (from 0 to 1) providing maximum roll, pitch and yaw range without climbing
//      b) thr_adj: the difference between the pilot's desired throttle and throttle_thrust_best_rpy
//   records filtered motor output values in _thrust_rpyt_out_filt array
//   sets thrust_balanced to true if motors are balanced, false if a motor failure is detected
//   sets _motor_lost_index to index of failed motor
void                mb_motors_check_for_failed_motor(mb_motors_t motors, float throttle_thrust_best_plus_adj);

// normalizes the roll, pitch and yaw factors so maximum magnitude is 0.5
// normalizes throttle factors so max value is 1 and no value is less than 0
void                mb_motors_normalise_rpy_factors(mb_motors_t motors);

void                mb_motors_bind_motors_mc_function(mb_motors_mc_ops_t ops_frontend, mb_motors_mc_ops_t ops_motors_matrix);

/// 
void                mb_motors_SetupMotors(mb_motors_t motors, enum mb_motor_frame_class frame_class, enum mb_motor_frame_type frame_type);

/// 
// configures the motors for the defined frame_class and frame_type
static inline void  mb_motors_setup_motors(mb_motors_t motors, enum mb_motor_frame_class frame_class, enum mb_motor_frame_type frame_type) {
    if (((mb_motors_mat_t)motors)->ops_mat->setup_motors) {
        ((mb_motors_mat_t)motors)->ops_mat->setup_motors(motors, frame_class, frame_type);
        return;
    }

    mb_motors_SetupMotors(motors, frame_class, frame_type);
}

/*------------------------------------test------------------------------------*/

#ifdef __cplusplus
}
#endif



